/*
 * Copyright (c) 2022 HiSilicon (Shanghai) Technologies CO., LIMITED.
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <math.h>
#include <stdlib.h>
#include <string.h> // For memcpy

#include <hi_time.h>

#include <stdio.h>
#include <unistd.h>
#include "iot_gpio_ex.h"

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_uart.h"
#include "hi_uart.h"
#include "iot_i2c.h"
#include "iot_errno.h"
#include "hi_errno.h"
#include "hi_i2c.h"
#include "iot_gpio.h"
#include "ssd1306_fonts.h"
#include "ssd1306.h"
#include "gyro.h"

// 定义接收来自GPS数据的数组长度1024
#define UART_BUFF_SIZE 1024
#define U_SLEEP_TIME 1000000
#define U_WAITING_TIME 100000
#define LSM6DS_I2C_IDX 0
#define IOT_I2C_IDX_BAUDRATE 400000 // 400k

#define Kp 20.0f /* 20.0f 比例增益支配率收敛到加速度计/磁强计 Proportional gain dominance converges to accelerometer/magnetometer */
/* 0004f 积分增益支配率的陀螺仪偏见的衔接 The Connection of Gyroscope Bias with Integrated Gain Dominance */
#define Ki 0.0004f
/* 0.005f 采样周期的一半 Half of the sampling period */
#define halfT 0.005f
/* 1 0 0 0四元数的元素，代表估计方向 Element of quaternion, representing estimation direction */
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
/* 按比例缩小积分误差 Scale down integral error */
float exInt = 0, eyInt = 0, ezInt = 0;
/* 偏航角，俯仰角，翻滚角 Yaw angle */
float Yaw, Pitch, Roll;
/* 0.0f 偏航角初始值 0.0f initial yaw angle */
static float yaw_conv = 0.0f;

/*
 * @berf i2c read
 * @param hi_u8 reg_high_8bit_cmd:Transmit register value 8 bits high
 * @param hi_u8 reg_low_8bit_cmd:Transmit register value low 8 bits
 * @param hi_u8* recv_data:Receive data buff
 * @param hi_u8 send_len:Sending data length
 * @param hi_u8 read_len:Length of received data
 */
uint32_t LSM6DS_WriteRead(uint8_t reg_high_8bit_cmd, uint8_t send_len, uint8_t read_len)
{
    uint32_t status = 0;
    uint8_t recvData[888] = {0};
    uint32_t ret = 0;
    hi_i2c_data c081nfc_i2c_write_cmd_addr = {0};
    uint8_t send_user_cmd[1] = {reg_high_8bit_cmd};

    memset(recvData, 0x0, sizeof(recvData));
    c081nfc_i2c_write_cmd_addr.send_buf = send_user_cmd;
    c081nfc_i2c_write_cmd_addr.send_len = send_len;

    c081nfc_i2c_write_cmd_addr.receive_buf = recvData;
    c081nfc_i2c_write_cmd_addr.receive_len = read_len;

    status = hi_i2c_writeread(LSM6DS_I2C_IDX, LSM6DS_READ_ADDR, &c081nfc_i2c_write_cmd_addr);
    if (status != IOT_SUCCESS)
    {
        return status;
    }
    ret = recvData[0];
    return ret;
}

uint32_t LSM6DS_ReadCont(uint8_t reg_addr, uint8_t *buffer, uint16_t read_len)
{
    uint32_t status;
    hi_i2c_data i2c_attr;

    i2c_attr.send_buf = &reg_addr;
    i2c_attr.send_len = 1;
    i2c_attr.receive_buf = buffer;
    i2c_attr.receive_len = read_len;

    status = hi_i2c_writeread(LSM6DS_I2C_IDX, LSM6DS_READ_ADDR, &i2c_attr);

    return status;
}

static uint32_t LSM6DS_Write(uint8_t addr, uint8_t writedata, uint32_t buffLen)
{
    uint8_t buffer[2] = {addr, writedata}; // 2代表buff长度 2 represents buff length
    uint32_t retval = IoTI2cWrite(LSM6DS_I2C_IDX, LSM6DS_WRITE_ADDR, buffer, buffLen);
    if (retval != IOT_SUCCESS)
    {
        // printf("IoTI2cWrite(%02X) failed, %0X!\n", buffer[0], retval);
        return retval;
    }
    return IOT_SUCCESS;
}

void GetLSM6DS(void)
{
    while (1)
    {
        TaskMsleep(5); // 5ms
        Lsm_Get_RawAcc();
    }
}

void LSM6DS_Init(void)
{
    /* 0x34 2初始化陀螺仪 0x34 2 Initialize gyroscope */
    LSM6DS_Write(LSM6DSL_CTRL3_C, 0x34, 2);
    /* 0X4C 2 配置陀螺仪 0X4C 2 Configure gyroscope */
    LSM6DS_Write(LSM6DSL_CTRL2_G, 0X4C, 2); // 角速度陀螺仪配置2000dps ,104Hz
    /* 0x38 2  timer en, pedo en, tilt en */
    LSM6DS_Write(LSM6DSL_CTRL10_C, 0x38, 2);
    /* 0x4F 2 加速度配置量程为8g,104Hz, lpf1_bw_sel=1, bw0_xl=1; */
    /* 0x4F 2 The acceleration configuration range is 8g, 104Hz, lpf1_ bw_ sel=1, bw0_ xl=1; */
    LSM6DS_Write(LSM6DSL_CTRL1_XL, 0x4F, 2);
    /* 0x10 2  */
    LSM6DS_Write(LSM6DSL_TAP_CFG, 0x10, 2);
    /* 0x00 2  */
    LSM6DS_Write(LSM6DSL_WAKE_UP_DUR, 0x00, 2);
    /* 0x02 2  */
    LSM6DS_Write(LSM6DSL_WAKE_UP_THS, 0x02, 2);
    /* 0x40 2  */
    LSM6DS_Write(LSM6DSL_TAP_THS_6D, 0x40, 2);
    /* 0x01 2  */
    LSM6DS_Write(LSM6DSL_CTRL8_XL, 0x01, 2);
}

/**********************************************************************************************************************/
void Uart1GpioInit(void)
{
    IoTGpioInit(IOT_IO_NAME_GPIO_0);

    // 设置GPIO0的管脚复用关系为UART1_TX Set the pin reuse relationship of GPIO0 to UART1_ TX
    IoSetFunc(IOT_IO_NAME_GPIO_0, IOT_IO_FUNC_GPIO_0_UART1_TXD);
    IoTGpioInit(IOT_IO_NAME_GPIO_1);

    // 设置GPIO1的管脚复用关系为UART1_RX Set the pin reuse relationship of GPIO1 to UART1_ RX
    IoSetFunc(IOT_IO_NAME_GPIO_1, IOT_IO_FUNC_GPIO_1_UART1_RXD);
}

/**********************************************************************************************************************/

void Uart1Config(void)
{
    uint32_t ret;

    /* 初始化UART配置，波特率 9600，数据bit为8,停止位1，奇偶校验为NONE */
    /* Initialize UART configuration, baud rate is 9600, data bit is 8, stop bit is 1, parity is NONE */
    IotUartAttribute uart_attr = {
        .baudRate = 9600,
        .dataBits = 8,
        .stopBits = 1,
        .parity = 0,
    };

    ret = IoTUartInit(HI_UART_IDX_1, &uart_attr);

    if (ret != IOT_SUCCESS)
    {
        //printf("Init Uart1 Falied Error No : %d\n", ret);
        return;
    }
}

void Lsm_Get_RawAcc(void)
{
    uint8_t buf[12] = {0};
    int16_t acc_x = 0, acc_y = 0, acc_z = 0;
    float acc_x_conv = 0, acc_y_conv = 0, acc_z_conv = 0;
    int16_t ang_rate_x = 0, ang_rate_y = 0, ang_rate_z = 0;
    float ang_rate_x_conv = 0, ang_rate_y_conv = 0, ang_rate_z_conv = 0;
    float ang_rate_x_cal = 0, ang_rate_y_cal = 0, ang_rate_z_cal = 0;
    char timeStr[50];

    char latitudeStr[50];

    char longtitudeStr[50];

    char altitudeStr[50];

    char *nmea_fields[13]; //$GPGGA所在的行有13个数
    const char *data = "Hello OpenHarmony !!!\n";
    uint32_t count = 0;
    uint32_t len = 0;
    unsigned char uartReadBuff[UART_BUFF_SIZE] = {0};

    // 对UART1的一些初始化 Some initialization of UART1
    Uart1GpioInit();
    // 对UART1参数的一些配置 Some configurations of UART1 parameters
    Uart1Config();
    if ((LSM6DS_WriteRead(LSM6DSL_STATUS_REG, 1, 1) & 0x03) != 0)
    {
        if (IOT_SUCCESS != LSM6DS_ReadCont(LSM6DSL_OUTX_L_G, buf, 12))
        { // 12buf长度
            printf("i2c read error!\n");
        }
        else
        {
            ang_rate_x = (buf[1] << 8) + buf[0]; // buf第1位左移8位与buff第0位
            ang_rate_y = (buf[3] << 8) + buf[2]; // buf第3位左移8位与buff第2位
            ang_rate_z = (buf[5] << 8) + buf[4]; // buf第5位左移8位与buff第4位
            acc_x = (buf[7] << 8) + buf[6];      // buf第7位左移8位与buff第6位
            acc_y = (buf[9] << 8) + buf[8];      // buf第9位左移8位与buff第8位
            acc_z = (buf[11] << 8) + buf[10];    // buf第11位左移8位与buff第10位

            ang_rate_x_conv = (3.14 * ang_rate_x) / 180; // 3.14= π 180 °
            ang_rate_y_conv = (3.14 * ang_rate_y) / 180; // 3.14= π 180 °
            ang_rate_z_conv = (3.14 * ang_rate_z) / 180; // 3.14= π 180 °
            ang_rate_x_cal = ang_rate_x_conv / 14.29;    // 14.29量程
            ang_rate_y_cal = ang_rate_y_conv / 14.29;    // 14.29量程
            ang_rate_z_cal = ang_rate_z_conv / 14.29;    // 14.29量程
            acc_x_conv = acc_x / 4098.36;                // 4098.36量程
            acc_y_conv = acc_y / 4098.36;                // 4098.36量程
            acc_z_conv = acc_z / 4098.36;                // 4098.36量程
            //printf("lsm trans acc: %.2f, %.2f, %.2f \n ang: %.2f, %.2f, %.2f, ang_cal: %.2f, %.2f, %.2f\n ", acc_x_conv, acc_y_conv, acc_z_conv, ang_rate_x_conv, ang_rate_y_conv, ang_rate_z_conv, ang_rate_x_cal, ang_rate_y_cal, ang_rate_z_cal);
            if (acc_z_conv > 1.5)
            {
                while (1)
                {

                    // 通过UART1 发送数据 Send data through UART1,即3861向GPS模块发送数据的部分。
                    // IoTUartWrite(HI_UART_IDX_1, (unsigned char*)data, strlen(data));

                    // 通过UART1 接收数据 Receive data through UART1
                    usleep(U_SLEEP_TIME);
                    len = IoTUartRead(HI_UART_IDX_1, uartReadBuff, UART_BUFF_SIZE);
                    if (len > 0)
                    {
                        // 把接收到来自GPS模块的数据打印出来 Print the received data

                        printf("\r\n--------------------------------------------------------------\r\n");
                        printf("[ %d ]\r\n", count);
                        printf("The serial port information output by the GPS module is : \r\n");
                        printf("%s \r\n", uartReadBuff);

                        /***********************************解析GPS模块发送的 NMEA 0183协议的数据帧**************************************/

                        /********************获取$GNGGA的经纬度信息（WGS-84国际标准下）***************************************/
                        // 查找$GNGGA所在的行
                        char *nmea_gngga = strstr(uartReadBuff, "$GPGGA");
                        if (nmea_gngga == NULL)
                        {
                            printf("[$GNGGA,WGS-84]The information on the $GNGGA line not found\n");
                        }
                        else
                        {
                            printf("[$GNGGA,WGS-84]The information on the $GNGGA line is found\n");
                        }

                        // 使用strtok函数分割 $GNGGA所在的行
                        char *token;
                        token = strtok(nmea_gngga, ",");

                        int i = 0;
                        while (token != NULL)
                        {
                            nmea_fields[i++] = token;
                            token = strtok(NULL, ",");
                            if (i >= 13)
                                break;
                        }
                        printf("UTC Time: %s\n", nmea_fields[1]);
                        printf("Latitude: %s %c\n", nmea_fields[2], nmea_fields[3][0]);
                        printf("Longitude: %s %c\n", nmea_fields[4], nmea_fields[5][0]);
                        printf("Fix Quality: %s\n", nmea_fields[6]);
                        printf("Number of Satellites: %s\n", nmea_fields[7]);
                        printf("HDOP: %s\n", nmea_fields[8]);
                        printf("Altitude: %s %c\n", nmea_fields[9], nmea_fields[10][0]);
                        printf("Geoidal Separation: %s %c\n", nmea_fields[11], nmea_fields[12][0]);

                        usleep(U_SLEEP_TIME);

                        snprintf(timeStr, sizeof(timeStr), "UTC Time: %s\n", nmea_fields[1]);
                        IoTUartWrite(HI_UART_IDX_1, (unsigned char *)timeStr, strlen(timeStr));
                        usleep(U_WAITING_TIME);
                        snprintf(latitudeStr, sizeof(latitudeStr), "Latitude: %s %c\n", nmea_fields[2], nmea_fields[3][0]);
                        IoTUartWrite(HI_UART_IDX_1, (unsigned char *)latitudeStr, strlen(latitudeStr));
                        usleep(U_WAITING_TIME);
                        snprintf(longtitudeStr, sizeof(longtitudeStr), "Longitude: %s %c\n", nmea_fields[4], nmea_fields[5][0]);
                        IoTUartWrite(HI_UART_IDX_1, (unsigned char *)longtitudeStr, strlen(longtitudeStr));
                        usleep(U_WAITING_TIME);
                        snprintf(altitudeStr, sizeof(altitudeStr), "Altitude: %s %c\n", nmea_fields[9], nmea_fields[10][0]);
                        IoTUartWrite(HI_UART_IDX_1, (unsigned char *)altitudeStr, strlen(altitudeStr));
                        usleep(U_SLEEP_TIME);
                        count++;
                    }
                }
            }
        }
    }
}